#include "hw_jy901s.h"
#include "stdio.h"
#include "string.h"

#define I2C_DELAY_TIME 5   //I2C的基本时序时间

GYRO_ANGLE_DATA_STRUCT angle_struct;

static void i2c_start(void)
{
        SDA_OUT();

        SCL(0);
        SDA(1);
        SCL(1);

        delay_us(I2C_DELAY_TIME);

        SDA(0);
        delay_us(I2C_DELAY_TIME);
        SCL(0);
        delay_us(I2C_DELAY_TIME);
}

static void i2c_stop(void)
{
        SDA_OUT();
        SCL(0);
        SDA(0);

        SCL(1);
        delay_us(I2C_DELAY_TIME);
        SDA(1);
        delay_us(I2C_DELAY_TIME);

}

/******************************************************************
 * 函 数 说 明：主机发送应答或者非应答信号
 * 函 数 形 参：0发送应答  1发送非应答
******************************************************************/
static void i2c_send_ack(unsigned char ack)
{
        SDA_OUT();
        SCL(0);
        SDA(0);
        delay_us(I2C_DELAY_TIME);
        if(!ack) SDA(0);
        else     SDA(1);
        SCL(1);
        delay_us(I2C_DELAY_TIME);
        SCL(0);
        SDA(1);
}


/******************************************************************
 * 函 数 说 明：等待从机应答
 * 函 数 返 回：0有应答  1超时无应答
******************************************************************/
static unsigned char i2c_wait_ack(void)
{
        char ack = 0;
        char ack_flag = 50;
        SDA_IN();
        SDA(1);
        while( (SDA_GET()==1) && ( ack_flag ) )
        {
                ack_flag--;
                delay_us(I2C_DELAY_TIME);
        }

        if( ack_flag == 0 )
        {
                i2c_stop();
                return 1;
        }
        else
        {
                SCL(1);
                delay_us(I2C_DELAY_TIME);
                SCL(0);
                SDA_OUT();
        }
        return ack;
}

/******************************************************************
 * 函 数 说 明：写入一个字节
 * 函 数 形 参：dat要写人的数据
******************************************************************/
static void i2c_send_byte(uint8_t dat)
{
        int i = 0;
        SDA_OUT();
        SCL(0);//拉低时钟开始数据传输

        for( i = 0; i < 8; i++ )
        {
                SDA( (dat & 0x80) >> 7 );
                delay_us(2);
                SCL(1);
                delay_us(I2C_DELAY_TIME);
                SCL(0);
                delay_us(I2C_DELAY_TIME);
                dat<<=1;
        }
}

/******************************************************************
 * 函 数 说 明：IIC读时序
 * 函 数 返 回：读到的数据
******************************************************************/
static unsigned char i2c_read_byte(void)
{
    unsigned char i,receive=0;
    SDA_IN();//SDA设置为输入
    for(i=0;i<8;i++ )
    {
        SCL(0);
        delay_us(I2C_DELAY_TIME);
        SCL(1);
        delay_us(I2C_DELAY_TIME);
        receive<<=1;
        if( SDA_GET() )
        {
            receive|=1;
        }
        delay_us(I2C_DELAY_TIME);
    }

    SCL(0);

    return receive;
}

/******************************************************************
 * 函 数 名 称：jy901s_write_reg
 * 函 数 说 明：I2C连续写入数据
 * 函 数 形 参：addr器件地址 regaddr寄存器地址 num要写入的长度 regdata写入的数据地址
 * 函 数 返 回：0=读取成功   其他=读取失败
 * 作       者：LC
 * 备       注：无
******************************************************************/
char jy901s_write_reg(uint8_t addr, uint8_t regaddr, uint8_t num, uint8_t* regdata)
{
    uint16_t i = 0;
    i2c_start();
    i2c_send_byte((addr<<1));
    if( i2c_wait_ack() == 1 ) {i2c_stop();return 1;}
    i2c_send_byte(regaddr);
    if( i2c_wait_ack() == 1 ) {i2c_stop();return 2;}

    for( i = 0; i < num; i++ )
    {
        i2c_send_byte(regdata[i]);
        if( i2c_wait_ack() == 1 ) {i2c_stop();return (3+i);}
    }
    i2c_stop();
    return 0;
}


/******************************************************************
 * 函 数 名 称：jy901s_read_data
 * 函 数 说 明：I2C连续读取数据
 * 函 数 形 参：addr器件地址 regaddr寄存器地址 num要读取的长度 Read读取到的数据要存储的地址
 * 函 数 返 回：0=读取成功   其他=读取失败
 * 作       者：LC
 * 备       注：无
******************************************************************/
char jy901s_read_data(uint8_t addr, uint8_t regaddr,uint8_t num,uint8_t* Read)
{
        uint8_t i;
        i2c_start();
        i2c_send_byte((addr<<1));
        if( i2c_wait_ack() == 1 ) {i2c_stop();return 1;}
        i2c_send_byte(regaddr);
        if( i2c_wait_ack() == 1 ) {i2c_stop();return 2;}
        delay_us(5);

        i2c_start();
        i2c_send_byte((addr<<1)|1);
        if( i2c_wait_ack() == 1 ) {i2c_stop();return 3;}

        for(i=0;i<(num-1);i++)
        {
            Read[i]=i2c_read_byte();
            i2c_send_ack(0);
        }
        Read[i]=i2c_read_byte();
        i2c_send_ack(1);
        i2c_stop();
        return 0;
}


void jy901s_init(void)
{
        /*================Z轴归零==================*/
        // 寄存器解锁
        uint8_t unlock_reg1[2] = {0x88,0xB5};
        jy901s_write_reg(JY901S_ADDRESS,UN_REG,2, unlock_reg1);
        delay_ms(200);
        // Z轴归零
        uint8_t z_axis_reg[2] = {0x04,0x00};
        jy901s_write_reg(JY901S_ADDRESS,ANGLE_REFER_REG, 2, z_axis_reg);
        delay_ms(200);
        // 保存
        uint8_t save_reg1[2] = {0x00,0x00};
        jy901s_write_reg(JY901S_ADDRESS,SAVE_REG,2,save_reg1);
        delay_ms(200);

        /*================角度归零==================*/

        // 寄存器解锁
        uint8_t unlock_reg[2] = {0x88,0xB5};
        jy901s_write_reg(JY901S_ADDRESS,UN_REG,2, unlock_reg);
        delay_ms(200);
        // 角度归零
        uint8_t angle_reg[2] = {0x08,0x00};
        jy901s_write_reg(JY901S_ADDRESS,ANGLE_REFER_REG,2, angle_reg);
        delay_ms(200);
        // 保存
        uint8_t save_reg[2] = {0x00,0x00};
        jy901s_write_reg(JY901S_ADDRESS,SAVE_REG,2, save_reg);
        delay_ms(200);
}

/******************************************************************
 * 函 数 名 称：get_angle
 * 函 数 说 明：读角度数据
 * 函 数 形 参：无
 * 函 数 返 回：返回结构体
 * 作       者：LC
 * 备       注：无
******************************************************************/
float get_angle(void)
{
	// 数据缓存
	volatile uint8_t sda_angle[6] = {0};

	int ret = 0;

	// 读取寄存器数据
    // 从ROLL地址连续读取6个数据到sda_angle
	ret = jy901s_read_data(JY901S_ADDRESS, ROLL, 6, sda_angle);

	if(ret != 0)
	{
		// 读取失败
		printf("Read Error\r\n");
	}

	#if GYRO_DEBUG

	printf("RollL = %x\r\n",sda_angle[0]);
	printf("RollH = %x\r\n",sda_angle[1]);
	printf("PitchL = %x\r\n",sda_angle[2]);
	printf("PitchH = %x\r\n",sda_angle[3]);
	printf("YawL = %x\r\n",sda_angle[4]);
	printf("YawH = %x\r\n",sda_angle[5]);

	#endif

    // 计算 RollX, PitchY 和 YawZ 并确保它们在 -180 到 180 的范围内
    float RollX = (float)(((sda_angle[1] << 8) | sda_angle[0]) / 32768.0 * 180.0);
    if (RollX > 180.0)
    {
        RollX -= 360.0;
    }
    else if (RollX < -180.0)
    {
        RollX += 360.0;
    }

    float PitchY = (float)(((sda_angle[3] << 8) | sda_angle[2]) / 32768.0 * 180.0);
    if (PitchY > 180.0)
    {
        PitchY -= 360.0;
    }
    else if (PitchY < -180.0)
    {
        PitchY += 360.0;
    }

    float YawZ = (float)(((sda_angle[5] << 8) | sda_angle[4]) / 32768.0 * 180.0);
    if (YawZ > 180.0)
    {
        YawZ -= 360.0;
    }
    else if (YawZ < -180.0)
    {
        YawZ += 360.0;
    }

    // 将计算结果保存到结构体中
    angle_struct.x = RollX;
    angle_struct.y = PitchY;
    angle_struct.z = YawZ;

    // 返回角度数据
    return YawZ;
}